import json5
import json
import numpy as np
from new_json.jxq.part import Part
import math
import trimesh


def cosine_similarity(x, y):
    x=np.array(x)
    y=np.array(y)
    num = x.dot(y.T)
    denom = np.linalg.norm(x) * np.linalg.norm(y)
    return num / denom


def identity_matrix():
    return np.identity(4)


def long_line(v):
    """
    线段长度
    :return:
    """
    coordinates_squared = [x ** 2 for x in v]
    return math.sqrt(sum(coordinates_squared))


# 将向量归一化
def normalized(v):
    try:
        magnitude = long_line(v)
        return np.array([x * 1.0 / magnitude for x in v])
    except ZeroDivisionError:
        raise Exception('Cannot normalized the zero myVector2')


def make_group_assemble(input_josn, json_path,part_dir):
    assmeble_dict={}
    f=open(input_josn)
    assemble_data=json.load(f)
    f.close()
    assmeble_dict['version']=4.0
    assmeble_dict['elements']=[]
    elements=set()
    assemble_used_dict={}

    '''
    生成各自单元
    '''
    part_dict={}
    trans_dict={}
    for assemble in assemble_data:
        for part in ['FirstPart','SecondPart']:
            firstelement={}
            firstelement['category']=assemble[part]+'_stp'
            firstelement['name']=assemble[part[:-4]+'Name']
            firstelement['desc']=''
            if firstelement['name'] not in elements:
                elements.add(firstelement['name'])
                assmeble_dict['elements'].append(firstelement)
                assemble_used_dict[firstelement['name']]=False
                part_dict[firstelement['name']]=Part(firstelement['category'],part_dir=part_dir)
                trans_dict[firstelement['name']]=identity_matrix()


    '''
    生成装配
    '''

    assmeble_dict['trans']=[]
    for assemble in assemble_data:
        assemble_group={}
        assemble_group['group']=[]
        assemble_group['point2point']=[]
        pre_vertex=[]
        after_vertex=[]
        for v in assemble['PointRelation']:
            vertex0=part_dict[assemble['FirstName']].get_point(v[0])
            vertex1=part_dict[assemble['SecondName']].get_point(v[1])

            vertex0=trimesh.transform_points([vertex0], trans_dict[assemble['FirstName']])[0]
            vertex1=trimesh.transform_points([vertex1], trans_dict[assemble['SecondName']])[0]

            vertex0= np.append(vertex0,1)
            vertex1=np.append(vertex1,1)
            pre_vertex.append(np.array(vertex0))
            after_vertex.append(np.array(vertex1))

        v_list = assemble['PointRelation'][0]
        pre_normal = part_dict[assemble['FirstName']].get_normal('NL-' +v_list[0].split('-')[1])
        after_normal=part_dict[assemble['SecondName']].get_normal('NL-' +v_list[1].split('-')[1])

        pre_normal = trimesh.transform_points([pre_normal], trans_dict[assemble['FirstName'] ])[0]
        after_normal = trimesh.transform_points([after_normal], trans_dict[assemble['SecondName'] ])[0]


        assemble_group['point2point']=[assemble['FirstName']+'/'+v_list[0],assemble['SecondName']+'/'+v_list[1]]


        if assemble_used_dict[assemble['FirstName']]:
            assemble_group['group'].append(assemble['SecondName'])
            assemble_group['point2point'][0],assemble_group['point2point'][1]=assemble_group['point2point'][1],assemble_group['point2point'][0]
            pre_vertex,after_vertex=after_vertex,pre_vertex
            pre_normal,after_normal=after_normal,pre_normal
            need_hanele=assemble['SecondName']
        else:
            assemble_group['group'].append(assemble['FirstName'])
            need_hanele=assemble['FirstName']

        assemble_used_dict[assemble['SecondName']] = True
        assemble_used_dict[assemble['FirstName']] = True

        pre_vertex=np.asmatrix(pre_vertex)
        after_vertex=np.asmatrix(after_vertex)
        v1=(pre_vertex[1]-pre_vertex[0]).tolist()[0][:3]
        v2=(after_vertex[1]-after_vertex[0]).tolist()[0][:3]

        if np.abs(np.abs(cosine_similarity(v1, v2))-1)>1e-5:
            shape_angle = trimesh.transformations.angle_between_vectors(v1, v2,directed=True)
            shape_direction = trimesh.transformations.vector_product(v1, v2)
            rotate_matrix = trimesh.transformations.rotation_matrix(shape_angle, direction=shape_direction)
        elif np.abs(cosine_similarity(v1, v2)+1)<1e-5:
            rotate_matrix=trimesh.transformations.rotation_matrix(np.pi, direction=pre_normal)
        else:
            rotate_matrix=identity_matrix()

        pre_normal = normalized(pre_normal)

        pre_normal = trimesh.transform_points([pre_normal], rotate_matrix)[0]

        after_normal = -normalized(after_normal)

        angle_in_radians = math.acos(pre_normal.dot(after_normal))

        vertical_v = np.cross(pre_normal, after_normal)
        rotation_direction = True
        if vertical_v.dot(v2)>-1e-5:
            rotation_direction=True
        else:
            rotation_direction=False

        if rotation_direction:
            another_angle=angle_in_radians
        else:
            another_angle=-angle_in_radians
        rotate_matrix1=trimesh.transformations.rotation_matrix(another_angle,direction=v2)

        Transm=trimesh.transformations.concatenate_matrices(
            rotate_matrix1, rotate_matrix)

        trans_dict[need_hanele]=trimesh.transformations.concatenate_matrices(
                Transm, trans_dict[need_hanele])




            # ax, ay, az = trimesh.transformations.euler_from_matrix(rotate_matrix)
        rows,cols=Transm.shape
        m=[]
        for r in range(rows):
            newdata=[float(Transm[r,j]) for j in range(cols)]
            m.append(newdata)
        assemble_group['rotate']=[m,'+m']
        # print(trans[0:3,0:3])
        # rotate_list=rotationMatrixToEulerAngles(trans[0:3,0:3])
        # axis_dict={'x':0,'y':1,'z':2}
        # for k,v in axis_dict.items():
        #     angle=np.rad2deg(rotate_list[v])
        #     if abs(angle)>1e-5:
        #         rotate_group={}
        #         rotate_group['group']=assemble_group['group']
        #         if angle<0:
        #             angle+=360
        #         rotate_group['rotate']=[float(angle),"+"+k]
        #         assmeble_dict['trans'].append(rotate_group)
        assmeble_dict['trans'].append(assemble_group)

    json_file = open(json_path, 'w')
    print(assmeble_dict)
    json.dump(assmeble_dict, json_file)
    json_file.close()


if __name__ == '__main__':
    make_group_assemble(r'D:\Pycharm_Project\new_part\29-02\TEST2.json',
                        r'D:\Pycharm_Project\new_part\29-02\groupt.json5',
                        r'D:\Pycharm_Project\new_part\29-02')
